#ifndef MATRIX3_H
#define MATRIX3_H

#include "Prerequisites.h"
#include "Vector3D.h"

// NB All code adapted from Wild Magic 0.2 Matrix math (free source code)
// http://www.geometrictools.com/

// NOTE.  The (x,y,z) coordinate system is assumed to be right-handed.
// Coordinate axis rotation matrices are of the form
//   RX =    1       0       0
//           0     cos(t) -sin(t)
//           0     sin(t)  cos(t)
// where t > 0 indicates a counterclockwise rotation in the yz-plane
//   RY =  cos(t)    0     sin(t)
//           0       1       0
//        -sin(t)    0     cos(t)
// where t > 0 indicates a counterclockwise rotation in the zx-plane
//   RZ =  cos(t) -sin(t)    0
//         sin(t)  cos(t)    0
//           0       0       1
// where t > 0 indicates a counterclockwise rotation in the xy-plane.


/** A 3x3 matrix which can represent rotations around axes.
	@note
		<b>All the code is adapted from the Wild Magic 0.2 Matrix
		library (http://www.geometrictools.com/).</b>
	@par
		The coordinate system is assumed to be <b>right-handed</b>.
*/
class Matrix3
{
public:
	/** Default constructor.
		@note
			It does <b>NOT</b> initialize the matrix for efficiency.
	*/
	inline Matrix3 () {};
	inline explicit Matrix3 (const float arr[3][3])
	{
		memcpy(m,arr,9*sizeof(float));
	}
	inline Matrix3 (const Matrix3& rkMatrix)
	{
		memcpy(m,rkMatrix.m,9*sizeof(float));
	}
	Matrix3 (float fEntry00, float fEntry01, float fEntry02,
				float fEntry10, float fEntry11, float fEntry12,
				float fEntry20, float fEntry21, float fEntry22)
	{
		m[0][0] = fEntry00;
		m[0][1] = fEntry01;
		m[0][2] = fEntry02;
		m[1][0] = fEntry10;
		m[1][1] = fEntry11;
		m[1][2] = fEntry12;
		m[2][0] = fEntry20;
		m[2][1] = fEntry21;
		m[2][2] = fEntry22;
	}

	// member access, allows use of construct mat[r][c]
	inline float* operator[] (size_t iRow) const
	{
		return (float*)m[iRow];
	}
	/*inline operator float* ()
	{
		return (float*)m[0];
	}*/
	Vector3D GetColumn (size_t iCol) const;
	void SetColumn(size_t iCol, const Vector3D& vec);
	void FromAxes(const Vector3D& xAxis, const Vector3D& yAxis, const Vector3D& zAxis);

	// assignment and comparison
	inline Matrix3& operator= (const Matrix3& rkMatrix)
	{
		memcpy(m,rkMatrix.m,9*sizeof(float));
		return *this;
	}
	bool operator== (const Matrix3& rkMatrix) const;
	inline bool operator!= (const Matrix3& rkMatrix) const
	{
		return !operator==(rkMatrix);
	}

	// arithmetic operations
	Matrix3 operator+ (const Matrix3& rkMatrix) const;
	Matrix3 operator- (const Matrix3& rkMatrix) const;
	Matrix3 operator* (const Matrix3& rkMatrix) const;
	Matrix3 operator- () const;

	// matrix * vector [3x3 * 3x1 = 3x1]
	Vector3D operator* (const Vector3D& rkVector) const;

	// vector * matrix [1x3 * 3x3 = 1x3]
	friend Vector3D operator* (const Vector3D& rkVector,
		const Matrix3& rkMatrix);

	// matrix * scalar
	Matrix3 operator* (float fScalar) const;

	// scalar * matrix
	friend Matrix3 operator* (float fScalar, const Matrix3& rkMatrix);

	// utilities
	Matrix3 Transpose () const;
	bool Inverse (Matrix3& rkInverse, float fTolerance = 1e-06) const;
	Matrix3 Inverse (float fTolerance = 1e-06) const;
	float Determinant () const;

	// singular value decomposition
	void SingularValueDecomposition (Matrix3& rkL, Vector3D& rkS,
		Matrix3& rkR) const;
	void SingularValueComposition (const Matrix3& rkL,
		const Vector3D& rkS, const Matrix3& rkR);

	// Gram-Schmidt orthonormalization (applied to columns of rotation matrix)
	void Orthonormalize ();

	// orthogonal Q, diagonal D, upper triangular U stored as (u01,u02,u12)
	void QDUDecomposition (Matrix3& rkQ, Vector3D& rkD,
		Vector3D& rkU) const;

	float SpectralNorm () const;

	// matrix must be orthonormal
	void ToAxisAngle (Vector3D& rkAxis, Radian& rfAngle) const;
	inline void ToAxisAngle (Vector3D& rkAxis, Degree& rfAngle) const {
		Radian r;
		ToAxisAngle ( rkAxis, r );
		rfAngle = r;
	}
	void FromAxisAngle (const Vector3D& rkAxis, const Radian& fRadians);

	// The matrix must be orthonormal.  The decomposition is yaw*pitch*roll
	// where yaw is rotation about the Up vector, pitch is rotation about the
	// Right axis, and roll is rotation about the Direction axis.
	bool ToEulerAnglesXYZ (Radian& rfYAngle, Radian& rfPAngle,
		Radian& rfRAngle) const;
	bool ToEulerAnglesXZY (Radian& rfYAngle, Radian& rfPAngle,
		Radian& rfRAngle) const;
	bool ToEulerAnglesYXZ (Radian& rfYAngle, Radian& rfPAngle,
		Radian& rfRAngle) const;
	bool ToEulerAnglesYZX (Radian& rfYAngle, Radian& rfPAngle,
		Radian& rfRAngle) const;
	bool ToEulerAnglesZXY (Radian& rfYAngle, Radian& rfPAngle,
		Radian& rfRAngle) const;
	bool ToEulerAnglesZYX (Radian& rfYAngle, Radian& rfPAngle,
		Radian& rfRAngle) const;
	void FromEulerAnglesXYZ (const Radian& fYAngle, const Radian& fPAngle, const Radian& fRAngle);
	void FromEulerAnglesXZY (const Radian& fYAngle, const Radian& fPAngle, const Radian& fRAngle);
	void FromEulerAnglesYXZ (const Radian& fYAngle, const Radian& fPAngle, const Radian& fRAngle);
	void FromEulerAnglesYZX (const Radian& fYAngle, const Radian& fPAngle, const Radian& fRAngle);
	void FromEulerAnglesZXY (const Radian& fYAngle, const Radian& fPAngle, const Radian& fRAngle);
	void FromEulerAnglesZYX (const Radian& fYAngle, const Radian& fPAngle, const Radian& fRAngle);
	// eigensolver, matrix must be symmetric
	void EigenSolveSymmetric (float afEigenvalue[3],
		Vector3D akEigenvector[3]) const;

	static void TensorProduct (const Vector3D& rkU, const Vector3D& rkV,
		Matrix3& rkProduct);

	static const float EPSILON;
	static const Matrix3 ZERO;
	static const Matrix3 IDENTITY;

protected:
	// support for eigensolver
	void Tridiagonal (float afDiag[3], float afSubDiag[3]);
	bool QLAlgorithm (float afDiag[3], float afSubDiag[3]);

	// support for singular value decomposition
	static const float ms_fSvdEpsilon;
	static const unsigned int ms_iSvdMaxIterations;
	static void Bidiagonalize (Matrix3& kA, Matrix3& kL,
		Matrix3& kR);
	static void GolubKahanStep (Matrix3& kA, Matrix3& kL,
		Matrix3& kR);

	// support for spectral norm
	static float MaxCubicRoot (float afCoeff[3]);

	float m[3][3];

	// for faster access
	friend class Matrix4;

	/// Pointer accessor for direct copying
	inline float* ptr()
	{
		return (float*)m;
	}
	/// Pointer accessor for direct copying
	inline const float* ptr() const
	{
		return (float*)m;
	}
};

#endif
